#include "Spiral.h"

namespace OpenDrive {
	Spiral::Spiral(float s, const Point &startPosition, float heading, float length, float curvStart, float curvEnd) :\
								Geometry(s, startPosition, heading, length), curvStart(curvStart), curvEnd(curvEnd){}

	Spiral::~Spiral(){}

	void Spiral::calPosition(float step){
		double x = 0.0, y = 0.0, theta = getHeading(), heading = getHeading();
		double sx = getStartPosition().getX();
		double sy = getStartPosition().getY();
		float length = getLength();
		double cDot = (curvEnd - curvStart) / length;
		for (float dis = 0.0f; dis < length; dis += step) {
			eulerSpiral(sx, sy, curvStart, heading, cDot, dis, x, y, theta);
			Point p(x, y, theta, getS() + dis);
			addPoint(p);
		}
		//return getPoints();
	}
}